//sudo mount -t vboxsf share_d share/
//$CC qns7.c -o qns7_mc  -L ./  -lethercat -lpthread
//$CC qns7.c -o qns7_mc  -L ./  -lethercat -lpthread
//scp qns7_mc root@192.168.58.1:/home/mc

#include<stdio.h>
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <sched.h> /* sched_setscheduler() */
#include <pthread.h>
#include <math.h>

#include "ethercat.h"


/****************************************************************************

// Application parameters
#define FREQUENCY 1000
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define MEASURE_TIMING

/****************************************************************************

#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
        (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/****************************************************************************/



/** Task period in ns. */
#define PERIOD_NS   (1000000)

#define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is
                                     guranteed safe to access without
                                     faulting/*��֤��ȫ���ʶ�������������ջ��С */

                                     /****************************************************************************/

                                     /* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)

/****************************************************************************/
// EtherCAT
ec_master_t* master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t* domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t* domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};

static uint8_t* domainOutput_pd = NULL;
static uint8_t* domainInput_pd = NULL;

static ec_slave_config_t* sc_qns7;
static ec_slave_config_state_t sc_qns7_state;
/****************************************************************************/

#define qns7_pos0 0, 0
#define qns7 0x00000748, 0x00000006
// offsets for PDO entries
static unsigned int  cntlwd;
static unsigned int  tarpos;
static unsigned int  modope;
static unsigned int  status;
static unsigned int  actpos;
static unsigned int  moddis;
static unsigned int  fault_data;
static unsigned int follow_data;
static unsigned int db1,db2;


static int cur_status;
static int cur_mode;
static int cur_pos;

static unsigned int counter = 0;
static unsigned int blink = 0;

// process data
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    {qns7_pos0, qns7, 0x6040, 0x00, &cntlwd, NULL},//������
    {qns7_pos0, qns7, 0x607a, 0x00, &tarpos, NULL},//Ŀ��λ��
    {qns7_pos0, qns7, 0x6060, 0x00, &modope, NULL},	//����ģʽ�趨   
   // {qns7_pos0, qns7, 0x5ffe, 0x00, &db1, NULL},	//���ֽ� 
    {}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {qns7_pos0, qns7, 0x6041, 0x00, &status, NULL},//״̬��
    {qns7_pos0, qns7, 0x6064, 0x00, &actpos, NULL},//λ��ʵ��ֵ    
    {qns7_pos0, qns7, 0x6061, 0x00, &moddis, NULL},//����ģʽ��ʾ
   // {qns7_pos0, qns7, 0x5fff, 0x00, &db1, NULL},	//���ֽ�
    {qns7_pos0, qns7, 0x603f, 0x00, &fault_data, NULL},//���ϴ���
    {qns7_pos0, qns7, 0x60f4, 0x00, &follow_data, NULL},	//�������ֵ    
    {}
};

/* Master 0, Slave 0, "CoolDrive S7"
*Vendor ID : 0x00000748
* Product code : 0x00000006
* Revision number : 0x00000001
**/

static ec_pdo_entry_info_t qns7_pdo_entries[] = {
    {0x6040, 0x00, 16},//������
    {0x607a, 0x00, 32},//Ŀ��λ��
    {0x6060, 0x00, 8},//����ģʽ�趨
    {0x5ffe, 0x00, 8},//���ֽ�����
    {0x6041, 0x00, 16},//״̬��
    {0x6064, 0x00, 32},//λ��ʵ��ֵ
    {0x6077, 0x00, 16},//ת��ʵ��ֵ
    {0x6061, 0x00, 8},//����ģʽ��ʾ
    {0x5fff, 0x00, 8},//���ֽ�����
    {0x603f, 0x00, 16},//���ϴ���
    {0x60f4, 0x00, 32},//�������ֵ
};

static ec_pdo_info_t qns7_pdos[] = {
    {0x1600, 4, qns7_pdo_entries + 0},
    {0x1a00, 7, qns7_pdo_entries + 4},
};

static ec_sync_info_t qns7_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2,EC_DIR_OUTPUT, 1, qns7_pdos + 0, EC_WD_ENABLE},
    {3,EC_DIR_INPUT, 1, qns7_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

    
/*****************************************************************************
 * Realtime task
 ****************************************************************************/

void check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};
    //domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("domainServoInput: State %u.\n", ds.wc_state);
    }
    domainServoInput_state = ds;
    //domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter) {
        printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        printf("domainServoOutput: State %u.\n", ds1.wc_state);
    }
    domainServoOutput_state = ds1;
}

/****************************************************************************/

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }

    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }

    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/****************************************************************************/
void check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(sc_qns7, &s);
    if (s.al_state != sc_qns7_state.al_state)
        printf("sc_qns7_state: State 0x%02X.\n", s.al_state);
    if (s.online != sc_qns7_state.online)
        printf("sc_qns7_state: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_qns7_state.operational)
        printf("sc_qns7_state: %soperational.\n", s.operational ? "" : "Not ");
    sc_qns7_state = s;
}
void cyclic_task()
{
    // receive process data
    ecrt_master_receive(master);
    ecrt_domain_process(domainServoInput);//��վ����ֵ
    ecrt_domain_process(domainServoOutput);//��վд��ֵ

    // check process data state
    check_domain_state();

    if (counter) {
        counter--;
    }
    else { // do this at 1 Hz
        counter = FREQUENCY;

        // calculate new process data
        //blink = !blink;

        // check for master state (optional)
        check_master_state();

        // check for slave configuration state(s) (optional)
        check_slave_config_states();
    }

#if 1
    // read process data
    cur_status = EC_READ_U16(domainInput_pd + status);
    cur_pos = EC_READ_S32(domainInput_pd + actpos);
    printf("cur_status=%d,cur_pos=%d\n", cur_status, cur_pos);
#endif

#if 1
    // write process data
   
    EC_WRITE_U16(domainOutput_pd + cntlwd, 3);
    EC_WRITE_S32(domainOutput_pd + tarpos, 6);
    //EC_WRITE_U8(domainOutput_pd + modope, 5);
#endif

    // send process data
    ecrt_domain_queue(domainServoOutput);//��վд��ֵ
    ecrt_domain_queue(domainServoInput);//��վ����ֵ
    printf("before send\n");
    ecrt_master_send(master);
    printf("after send\n");
}
/**
void cyclic_task() {
    //ecrt_master_set_send_interval(master, 1000);
    ecrt_master_receive(master);
    printf("1 ok?\n");
    ecrt_domain_process(domainServoInput);//��վ����ֵ
    ecrt_domain_process(domainServoOutput);//��վд��ֵ
    printf("2 ok?\n");
    check_domain_state();    
    check_master_state();
    check_slave_config_states();   
    cur_status =EC_READ_U16(domainInput_pd + status);
    cur_pos = EC_READ_U32(domainInput_pd + actpos);
    printf("cur_status=%d,cur_pos=%d\n", cur_status,cur_pos);

    EC_WRITE_U16(domainOutput_pd + modope, 5);

    //EC_WRITE_U16(domainInput_pd,0x45);     	
    // send process data
    //domain_init();
   // clock_gettime(CLOCK_REALTIME, &tv);
    // write application time to master
   // ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
   // ecrt_master_sync_reference_clock(master);
   // ecrt_master_sync_slave_clocks(master);

    printf("3 ok?\n");
    ecrt_domain_queue(domainServoOutput);//��վд��ֵ
    ecrt_domain_queue(domainServoInput);//��վ����ֵ
    printf("ok?\n");
    
    ecrt_master_set_send_interval(master, 1000);
    ecrt_master_send(master);
    pause();

}
*****/

void stack_prefault(void)
{
    unsigned char dummy[MAX_SAFE_STACK];

    memset(dummy, 0, MAX_SAFE_STACK);
}
int main(void) {
   
    int ret = 0;
    struct timespec wakeup_time;
    master = ecrt_request_master(0);
    if (!master) {
        fprintf(stderr, "Failed to request master.\n");
        return -1;
    }
    printf("step1:request_master sucess\n");
    check_master_state();

    printf("step2:create domain.\n");
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }

    printf("step3:master get slave config\n");
    sc_qns7 =
        ecrt_master_slave_config(master, qns7_pos0, qns7);
    if (!sc_qns7) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }
    printf("step3.1:remap qn pdo.\n");
    
    if (ecrt_slave_config_pdos(sc_qns7, EC_END, qns7_syncs)) {
        fprintf(stderr, "Failed to configure PDOs.\n");
        return -1;
    }
    printf("step3.2:configure free runmode.\n");
    ecrt_slave_config_sdo16(sc_qns7, 0x1c32, 0x01, 0x00);//outtputͬ��������ͬ������
    ecrt_slave_config_sdo16(sc_qns7, 0x1c33, 0x01, 0x00);//inputͬ��������ͬ������
    printf("step2.2:configure SYNC signals.\n");
    ecrt_slave_config_dc(sc_qns7, 0, 4000000, 0, 0, 0);
    printf("step3.3:domain reg pdo entry.\n");
    
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    printf("step4:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master);
    sleep(5);
    printf("step4.1:check salve config states.\n");
    check_slave_config_states();
    ecrt_master_set_send_interval(master, 40000);
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
    /***************   
   ret = ecrt_master_select_reference_clock(master, sc_qns7);
   if (ret < 0) {
       fprintf(stderr, "Failed to select reference clock: %s\n",
           strerror(-ret));
       return ret;
   }
  ****************/
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain_in data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain_out data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");

    /* Set priority */

    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);

    printf("Using priority %i.\n", param.sched_priority);
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
        perror("sched_setscheduler failed");
    }
    /* Lock memory */
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        perror("mlockall failed");
        return -1;
    }

    stack_prefault();

    printf("Starting RT task with dt=%u ns.\n", PERIOD_NS);

    clock_gettime(CLOCK_MONOTONIC, &wakeup_time);
    wakeup_time.tv_sec += 1; /* start in future */
    wakeup_time.tv_nsec = 0;

    while (1) {
        ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
            &wakeup_time, NULL);
        if (ret) {
            fprintf(stderr, "clock_nanosleep(): %s\n", strerror(ret));
            break;
        }
        printf("next clcle\n");
        cyclic_task();
        printf("next clcles\n");
        wakeup_time.tv_nsec += PERIOD_NS;
        while (wakeup_time.tv_nsec >= NSEC_PER_SEC) {
            wakeup_time.tv_nsec -= NSEC_PER_SEC;
            wakeup_time.tv_sec++;
        }
    }

    return ret;


}


